﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Foundation;
using Drifting.MotionPlanning.Primitives;

namespace Drifting.MotionPlanning.Systems
{
    abstract public class BaseSystem
    {
        public class MemoryPad
        {
            public double[] k1;
            public double[] k2;
            public double[] k3;
            public double[] k4;
            public double[] Qd_in;
            public double[] Qd_out;
            public double[] q_tmp;

            public void Prepare(int dof_state)
            {
                k1 = new double[dof_state];
                k2 = new double[dof_state];
                k3 = new double[dof_state];
                k4 = new double[dof_state];
                Qd_in = new double[dof_state];
                Qd_out = new double[dof_state];
                q_tmp = new double[dof_state];
            }
        }

        protected int dof_state;
        protected int dof_alias;
        protected int dof_ctrl;
        protected double[] max_state;
        protected double[] min_state;
        protected double[] max_alias;
        protected double[] min_alias;
        protected double[] max_ctrl;
        protected double[] min_ctrl;
        protected double max_Dt;
        protected double min_Dt;

        protected double t_delta;

        public List<SimpleObs> SimpleEnv;

        public BaseSystem(string fileName)
        {
            t_delta = 0.001;
            SimpleEnv = new List<SimpleObs>();
            Load_StateSpace(fileName);
        }

        public int DOF_Alias
        { get { return this.dof_alias; } }

        public int DOF_State
        { get { return this.dof_state; } }

        public int DOF_Control
        { get { return this.dof_ctrl; } }

        public double[] Min_State
        { get { return min_state; } }

        public double[] Max_State
        { get { return max_state; } }

        public double[] Min_Alias
        { get { return min_alias; } }

        public double[] Max_Alias
        { get { return max_alias; } }

        public double[] Max_Control
        {
            get { return max_ctrl; }
        }

        public double[] Min_Control
        {
            get { return min_ctrl; }
        }

        virtual public void SampleState(Random rand, ref double[] q)
        {
            if (q == null)
                q = new double[dof_state];
            if (q.Length != dof_state)
                throw new ArgumentException();

            for (int i = 0; i < dof_state; ++i)
                q[i] = min_state[i] + rand.NextDouble() * (max_state[i] - min_state[i]);           
        }

        virtual public void SampleAlias(Random rand, ref double[] alias)
        {
            if (alias == null)
                alias = new double[dof_alias];
            if (alias.Length != dof_alias)
                throw new ArgumentException();

            for (int i = 0; i < dof_alias; ++i)
                alias[i] = min_alias[i] + rand.NextDouble() * (max_alias[i] - min_alias[i]);
        }

        public void SampleControl(Random rand, ref double[] ctrl, ref double Dt)
        {
            if (ctrl == null || ctrl.Length < dof_ctrl)
                ctrl = new double[dof_ctrl];

            for (int i = 0; i < dof_ctrl; ++i)
                ctrl[i] = min_ctrl[i] + rand.NextDouble() * (max_ctrl[i] - min_ctrl[i]);

            Dt = min_Dt + rand.NextDouble() * (max_Dt - min_Dt);
        }

        public double[] GenerateZeroState()
        {
            double[] q = new double[dof_state];
            for (int i = 0; i < dof_state; ++i)
                q[i] = (max_state[i] + min_state[i]) / 2;
            return q; 
        }

        public double[] GenerateZeroAlias()
        {
            double[] q = new double[dof_alias];
            for (int i = 0; i < dof_alias; ++i)
                q[i] = (max_alias[i] + min_alias[i]) / 2;
            return q;
        }

        virtual public bool Load_StateSpace(string fileName)
        {
            string rawText = TextAssistant.LoadFile(fileName);

            string[] separators = null;
            string[] entries = null;

            #region State Space
            separators = new string[] { "<DOF_State>", "</DOF_State>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.dof_state = int.Parse(entries[1]);

            separators = new string[] { "<DOF_Control>", "</DOF_Control>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.dof_ctrl = int.Parse(entries[1]);

            separators = new string[] { "<DOF_Alias>", "</DOF_Alias>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.dof_alias = int.Parse(entries[1]);

            separators = new string[] { "<Max_Alias>", "</Max_Alias>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.max_alias = TextAssistant.StringToDoubleArray(entries[1]);

            separators = new string[] { "<Min_Alias>", "</Min_Alias>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.min_alias = TextAssistant.StringToDoubleArray(entries[1]);

            separators = new string[] { "<Max_State>", "</Max_State>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.max_state = TextAssistant.StringToDoubleArray(entries[1]);

            separators = new string[] { "<Min_State>", "</Min_State>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.min_state = TextAssistant.StringToDoubleArray(entries[1]);

            separators = new string[] { "<Max_Ctrl>", "</Max_Ctrl>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.max_ctrl = TextAssistant.StringToDoubleArray(entries[1]);

            separators = new string[] { "<Min_Ctrl>", "</Min_Ctrl>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.min_ctrl = TextAssistant.StringToDoubleArray(entries[1]);

            separators = new string[] { "<Max_Dt>", "</Max_Dt>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.max_Dt = TextAssistant.StringToDoubleArray(entries[1])[0];

            separators = new string[] { "<Min_Dt>", "</Min_Dt>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.min_Dt = TextAssistant.StringToDoubleArray(entries[1])[0];

            #endregion


            #region Env
            separators = new string[] { "<Env>", "</Env>" };
            entries = rawText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            string EnvText = entries[1];

            separators = new string[] { "<Obs>", "</Obs>" };
            entries = EnvText.Split(separators, StringSplitOptions.None);

            int n = (entries.Length - 1) / 2;
            for (int i = 0; i < n; ++i)
            {
                string obsString = entries[1 + i * 2];
                SimpleRec obs = new SimpleRec();

                separators = new string[] { "<v>", "</v>" };
                string[] vs = obsString.Split(separators, StringSplitOptions.None);

                int m = (vs.Length - 1) / 2;
                for (int j = 0; j < m; ++j)
                {
                    string vString = vs[1 + j * 2];
                    obs.Add(TextAssistant.StringToDoubleArray(vString));
                }

                SimpleEnv.Add(obs);
            }
            
            #endregion


            return true;
        }

        virtual public Edge Steer(State x_start, State x_end)
        {
            throw new NotImplementedException();
        }

        virtual public Edge ExecuteControlTo(double[] q_target, double delta, State x_start, double duration)
        {
            throw new NotImplementedException();
        }

        virtual public bool Propagate(MemoryPad pad, double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            if (q_end == null)
                q_end = new double[dof_state];

            int n_step = (int)Math.Floor(duration / t_delta + 0.5);

            if (n_step == 0)
                throw new Exception("integration failure. too small t_delta?");

            MathAssistant.vector_set(q_start, ref pad.q_tmp);
            MathAssistant.vector_set(q_start, ref q_end);
            step_cost = 0;
            for (int i = 0; i < n_step; ++i)
            {
                this.Integration(pad, pad.q_tmp, u, t_delta, ref q_end);
                MathAssistant.vector_set(q_end, ref pad.q_tmp);
                step_cost += t_delta;
            }

            return inStateSpace(q_end);
        }

        abstract public void Qd(double[] q, double[] ctrl, ref double[] qd);

        virtual public void Integration(MemoryPad pad, double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {
            Qd(q, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k1);

            MathAssistant.vector_vlinear(pad.k1, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k2);

            MathAssistant.vector_vlinear(pad.k2, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k3);

            MathAssistant.vector_vlinear(pad.k3, 1, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MathAssistant.vector_kTimes(pad.Qd_out, t_delta, ref pad.k4);

            MathAssistant.vector_vlinear(pad.k1, 1.0 / 6.0, q, ref pad.Qd_out);
            MathAssistant.vector_vlinear(pad.k2, 1.0 / 3.0, pad.Qd_out, ref pad.Qd_in);
            MathAssistant.vector_vlinear(pad.k3, 1.0 / 3.0, pad.Qd_in, ref pad.Qd_out);
            MathAssistant.vector_vlinear(pad.k4, 1.0 / 6.0, pad.Qd_out, ref q_next);
        }

        virtual public bool inStateSpace(double[] q)
        {
            for (int i = 0; i<dof_state;++i )
            {
                if (q[i] < min_state[i] || q[i] > max_state[i])
                    return false;
            }
            return true;
        }

        virtual public bool ValidateEdge(Edge e)
        {
            return true;
        }

        public bool inAliasSpace(double[] a)
        {
            for (int i = 0; i < dof_alias; ++i)
            {
                if (a[i] < min_alias[i] || a[i] > max_alias[i])
                    return false;
            }
            return true;
        }

    }
}
